/*
 * Track.cpp
 *
 *  Created on: 18.10.2011
 *      Author: pavel
 */

#include "Track.h"

#include <list>
#include <vector>
#include <string>
#include <fstream>
#include <iostream>
#include <stdio.h>
#include <cmath>
#include <assert.h>

typedef std::vector<unsigned> Observation;

Track::Track() {
}

Track::~Track() {
}

void Track::addPoint(int x, int y) {
	points.push_back(Point(x, y));
}

void Track::save(const char * fileName) {
	ofstream fout;
	fout.open(fileName, ios::out);
	if (!fout) {
		std::cout << "Unable to open save " << fileName << "\n";
	} else {
		vector<Point>::iterator iter = points.begin();
		while (iter != points.end()) {
			fout << (*iter).x;
			fout << " ";
			fout << (*iter).y;
			fout << "\n";
			iter++;
		}
		fout.close();
	}
}

void Track::load(const char * fileName) {
	ifstream fin;
	fin.open(fileName);
	if (!fin) {
		std::cout << "Unable to load file " << fileName << "\n";
	} else {
		points.clear();
		while (!fin.eof()) {
			int x;
			int y;
			fin >> x >> y;
			points.push_back(Point(x, y));
		}
		fin.close();
	}
}

void Track::convert(Observation &obs, int degrees) const {
	size_t n = points.size();
	assert(n >= 2);
	assert(n < 1000);
	obs.resize(n);
	if (n >= 2) {
		Point p1 = 0;
		Point p2 = points[0];
		for (size_t i = 1; i < n; i++) {
			p1 = p2;
			p2 = points[i];
			int vx = p2.x - p1.x;
			int vy = p2.y - p1.y;
			double alfa = atan2(vx, vy) + 3.141592654;
			unsigned k = unsigned ((alfa / 2.0 / 3.141592654) * degrees) % degrees;
			obs[i] = k;
		}
	}
}


void Track::clear() {
	points.clear();
}

unsigned Track::size() {
	return points.size();
}

double Track::length() {
	if (points.size() >= 2) {
		double s = 0;
		int x0 = points[0].x;
		int y0 = points[0].y;
		for (unsigned i = 1; i < points.size(); i++) {
			int x = points[i].x;
			int y = points[i].y;
			double dx = x - x0;
			double dy = y - y0;
			s += sqrt(dx * dx + dy * dy);
			x0 = x;
			y0 = y;
		}
		return s;
	}
	return 0;
}

int Track::getX(unsigned index) {
	return (points.at(index)).x;
}

int Track::getY(unsigned index) {
	return (points.at(index)).y;
}

const vector<Point>& Track::getPoints() const {
	return points;
}

Point Track::getCenter() const {
	if (points.size() == 0) {
		return Point();
	}
	int minx, miny, maxx, maxy;
	minx = maxx = points[0].x;
	miny = maxy = points[0].y;
	for (size_t i = 1; i < points.size(); i++) {
		const int& x = points[i].x;
		const int& y = points[i].y;
		if (x < minx) {
			minx = x;
		} else if (x > maxx) {
			maxx = x;
		}
		
		if (y < miny) {
			miny = y;
		} else if (y > maxy) {
			maxy = y;
		}
	}
	return Point((maxx + minx) / 2, (maxy + miny) / 2);
}

